import serial
from time import sleep

#Muda o estado para host
#Essa configuracao eu fiz na mao
# 73 65 0D
#74 68 0D 73 73 0D


def Espera():
   sleep(.3) #utilizado esse tempo conforme TCC RoboWeb 300 milisegundos

voSerial = serial.Serial(
   port = 0,                             # Representa a porta COM6
   baudrate = 9600,                      # baud rate
    bytesize = serial.SEVENBITS,          # numero de databits (7 bits)
    parity = serial.PARITY_ODD,           # bit de paridade (Paridade IMPAR)
    stopbits= serial.STOPBITS_TWO,        # numero de bits de parada (2 stop bits )
    timeout = 1000,                       # ajusta um timeout (1000)
   rtscts = 1,                           # ativa controle de fluxo RTS/CTS
   writeTimeout = 1000                   # ajusta timeout de escrita (1000)
)


#Motor B e C - movimento vertical do pulso = OK
#Motor A - Abre e fecha a mao = OK
#Cotovelo D = OK
#Ombro E = ok

# ------ Move motor E -----
comando = "pd,E,300\r".encode("ascii")
voSerial.write(comando)

Espera() #FUNCAO RESPONSAVEL POR ESPERAR 300 MILISEGUNDOS

comando = "ms,E\r".encode("ascii")
voSerial.write(comando)

# ---------------------------

Espera()

# ------ Move motor D -----

comando = "pd,D,200\r".encode("ascii")
voSerial.write(comando)

Espera()

comando = "ms,D\r".encode("ascii")
voSerial.write(comando)

# -------------------------

Espera()

# ------ Move motor B -----

comando = "pd,B,600\r".encode("ascii")
voSerial.write(comando)

Espera()

comando = "ms,B\r".encode("ascii")
voSerial.write(comando)

# -------------------------

Espera()

# ------ Move motor C -----

comando = "pd,C,700\r".encode("ascii")
voSerial.write(comando)

Espera()

comando = "ms,C\r".encode("ascii")
voSerial.write(comando)

# --------------------------

Espera()

# ------ Move a GARRA go = Abrir -----
comando = "go\r".encode("ascii")
voSerial.write(comando)


sleep(5) #ESPERA 5 SEGUNDOS PARA TERMINAR O PROGRAMA
voSerial.close() #FECHA A CONEXAO SERIAL
